#include "board.h"
#include "MotorControl.h"
#include "pid.h"


#define PERIOD  100U  // ms
#define FACTOR  20000U


SoftTimer_TypeDef g_pid_tim;
PidController_TypeDef g_pid = {0};

extern float g_spd_m1;
int16_t g_s16PwmDuty;
int32_t g_s32SpdErr;


static void UpdatePID(void) {
    g_s32SpdErr = M1.ParamRW.s32d3SpeedTar - M1.ParamRO.s32d3SpeedFdbk / 20;
    
    g_spd_m1 = M1.ParamRO.s32d3SpeedFdbk / 1000.0 / 20;  // revs/sec
    
    PID_Update(&g_pid, g_s32SpdErr);
    
    g_s16PwmDuty = g_pid.s32Out / FACTOR;
    if (g_pid.s32Out < 0) {
        g_s16PwmDuty = 0;
    }
    HBridge_SetPwmDuty(M1.pstHBdg, g_s16PwmDuty);
}


void demo_pid(void) {
    Mot_InitSys();    
    SfTim_Init(&g_pid_tim, PERIOD*100U, UpdatePID);  // period 50ms
    
    // ===== init PID =====
    PID_Init(&g_pid, 100, 1, 0, PERIOD/10);
//    PID_Init(&g_pid, 800, 1, 0, 4);
    g_pid.s32IntegLimitUpper = 800 * FACTOR;
    g_pid.s32IntegLimitLower = 0;
    
    // ===== initial state =====
    M1.ParamRW.s32d3SpeedTar = 3 * 1000; 
    
    M1.pstHBdg->ParamChanged = true;
    M1.pstHBdg->Param.State = HB_State_Run;
    M1.pstHBdg->Param.DirMode = HB_DirMode_ALBHasForward;
    M1.pstHBdg->Param.Dir = HB_Dir_Backward;
    
    M1.pstEnc->ParamChanged = true;
    M1.pstEnc->Param.DirMode = Enc_DirMode_0;
    M1.pstEnc->Param.PulsPerRev = 26 - 1;
    
    while (1) {
        Mot_Update(&M1);        
    }
}
